package kit.edu.lego.gruppe3.levels.door;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.bluetooth.control.GateCommon;
import kit.edu.lego.gruppe3.bluetooth.control.GateControl;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.levels.LevelSolver;
import kit.edu.lego.gruppe3.levels.LevelSelector.Level;
import kit.edu.lego.gruppe3.levels.labyrinth.FindLeftWall;
import kit.edu.lego.gruppe3.levels.labyrinth.LabyrinthSolver;
import kit.edu.lego.gruppe3.levels.rollbridge.RollBridgeSolver;
import kit.edu.lego.gruppe3.levels.sumpf.SumpfSolver;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class DoorSolver implements LevelSolver
{
	
	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();

	@Override
	public void start()
	{
		finishPreviousLevels();
		LevelSelector.getInstance().setCurrentLevel(Level.firstDoor);
		// connect bluetooth and open door
		gateControl = new GateControl();
		
		Behavior rightWall = new MoveToRightWall();
		Behavior doorMove = new DoorMove();
		Behavior doorEnd = new DoreEnd();
		Behavior hitWall = new HitFrontWall();
		
		Behavior[] bs = { doorMove, rightWall, hitWall, doorEnd};
		Arbitrator arby = new Arbitrator(bs);
		
		
		
		ComponentsRepository.getInstance().getMiddleMotor().rotate(90);
		ComponentsRepository.getInstance().getMiddleMotor().stop();
		
		robot.rotate(90);
		
		while (!ComponentsRepository.getInstance().getLeftTouchSensor().isPressed() &&
				!ComponentsRepository.getInstance().getRightTouchSensor().isPressed())
		{
			robot.forward();
		}
		
		robot.travel(-2.3f);
		robot.rotate(-90);
		robot.travel(5f);
		
		while ( !gateControl.connectionToGateSuccessful(GateCommon.GATE_1));
		if (gateControl.openGate())
		{	
			arby.start();
		}
		
		
		
			
	}
	
	private static GateControl gateControl = null;
	
	public static void closeGate()
	{
		if (gateControl != null)
		{
			gateControl.disconnectFromGate();
		}
	}
	
	private void finishPreviousLevels()
	{
		LabyrinthSolver.setLevelCleared(true);
		SumpfSolver.setLevelCleared(true);
		RollBridgeSolver.setLevelCleared(true);
	}
	
	private static boolean levelCleared = false;
	
	public static boolean isLevelCleared()
	{
		return levelCleared;
	}
	
	public static void setLevelCleared(boolean cleared)
	{
		levelCleared = cleared;
	}
	
	@Override
	public String toString()
	{
		return "Door solver";
	}

}
